#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <mother_ship/MyIMU.h>
#include <stdio.h>
#include <iostream>
#include <sstream>

using namespace std;

mother_ship::MyIMU imu_my_msg;

ros::Publisher imu_data;
sensor_msgs::Imu imu_msg;

ros::Subscriber imu_my_sub;

int imu_data_count;

void imu_my_Callback(const mother_ship::MyIMU::ConstPtr &msg) {
	boost::array<double, 9>
			cov = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
	imu_msg.angular_velocity.x = msg->angleacc_x;
	imu_msg.angular_velocity.y = msg->angleacc_y;
	imu_msg.angular_velocity.z = msg->angleacc_z;
	imu_msg.angular_velocity_covariance = cov;
	imu_msg.linear_acceleration.x = msg->acc_x;
	imu_msg.linear_acceleration.y = msg->acc_y;
	imu_msg.linear_acceleration.z = msg->acc_z;
	imu_msg.linear_acceleration_covariance = cov;
	imu_msg.orientation.w = msg->gyro_w;
	imu_msg.orientation.x = msg->gyro_x;
	imu_msg.orientation.y = msg->gyro_y;
	imu_msg.orientation.z = msg->gyro_z;
	imu_msg.orientation_covariance = cov;
	imu_data.publish(imu_msg);
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "topic_transit");
	ros::NodeHandle n;
	imu_data_count = 0;

	imu_data = n.advertise<sensor_msgs::Imu> ("imu_topic", 1);

	imu_my_sub = n.subscribe("imu_my", 1, imu_my_Callback);

	ros::Rate loop_rate(10);
	//while (1) {
	//	ros::spin();
	//	loop_rate.sleep();
	//}
	ros::spin();
}
